# !/usr/bin/env python
# -*- coding: UTF-8 -*-
import cv2
import numpy as np
import sys
import math


# 声明全局变量
global blue_lower_line
global blue_upper_line
global kernel_set_blue_line
global cap_camera_wig

global K_blue
global threshold_2


# 一个初始化全局变量的函数
def init_detect():
    global blue_lower_line
    global blue_upper_line
    global kernel_set_blue_line
    global cap_camera_wig
    global K_blue
    global threshold_2

    # 下面是三个圆形标志检测蓝色部分用到的HSV掩膜范围
    # blue_lower_line = np.array([100, 110, 50])
    # blue_upper_line = np.array([129, 250, 255])
    blue_lower_line = np.array([90, 50, 50])
    blue_upper_line = np.array([129, 250, 255])
    # 生成用于形态学操作的矩阵,这里专门用于进行蓝线处理
    kernel_set_blue_line = cv2.getStructuringElement(shape=cv2.MORPH_RECT,
                                                     ksize=(3, 3),
                                                     anchor=(-1, -1)
                                                     )
    # 这个是定义检测直线精度的变量，默认值为1，越大，精度越低
    K_blue = 0.8

    # 这里设定检测直线所用到的累加器阈值，越大，则检测到的直线越长
    threshold_2 = 70


# 适用于巡线排除干扰的蓝色原始掩膜
def original_blue_mask(blue_img):
    global blue_lower_line
    global blue_upper_line
    global kernel_set_blue_line

    image_HSV = cv2.cvtColor(src=blue_img, code=cv2.COLOR_BGR2HSV)
    blue_original_mask = cv2.inRange(src=image_HSV,
                                     lowerb=blue_lower_line,
                                     upperb=blue_upper_line)
    # cv2.imshow('blue_mask:', blue_line_mask)

    return blue_original_mask


# 找到经过处理的蓝色线条掩膜
def find_blue_line_mask(img_blue_line):
    global blue_lower_line
    global blue_upper_line
    global kernel_set_blue_line

    image_HSV = cv2.cvtColor(src=img_blue_line, code=cv2.COLOR_BGR2HSV)
    blue_line_mask = cv2.inRange(src=image_HSV,
                                 lowerb=blue_lower_line,
                                 upperb=blue_upper_line)
    # cv2.imshow('blue_mask:', blue_line_mask)
    # ----------------------------------------------------------这里进行腐蚀运算的操作
    # 进行腐蚀运算操作
    blue_line_mask = cv2.morphologyEx(src=blue_line_mask,
                                      op=cv2.MORPH_ERODE,
                                      kernel=kernel_set_blue_line,
                                      # 腐蚀的采用卷积核大小为3*3，腐蚀次数为5
                                      iterations=5)
    # cv2.imshow('open_cal:', blue_line_mask)
    # ----------------------------------------------------------------------------------

    return blue_line_mask


# 这里定义摄像头畸变矫正函数
def Straighten_the_camera(original_img):
    # 这里对摄像头的视频帧进行矫正
    # 定义相机内参矩阵，畸变矩阵，用来矫正相机畸变：相机型号HF867
    CamMax = np.array([[367.87792233, 0, 320.00047236],
                       [0, 370.36759206, 240.00000000],
                       [0, 0, 1]])

    disCef = np.array([-0.3369, 0.0870, 0, 0, 0])

    # 这里就是矫正摄像头畸变的操作
    global und_st
    und_st = cv2.undistort(src=original_img,
                           cameraMatrix=CamMax,
                           distCoeffs=disCef,
                           newCameraMatrix=CamMax
                           )
    # 将矫正后的图片返回出去
    return und_st


# 这里定义绘制第二种直线数据的函数
def draw_line_2(img, line_draw):
    if line_draw is not None:
        # 进行直线的绘制
        draw_line = img.copy()
        # for m in range(0, 1):
        # 绘制直线的序号
        n = 0
        cv2.line(img=draw_line,
                 pt1=(line_draw[n][0][0], line_draw[n][0][1]),
                 pt2=(line_draw[n][0][2], line_draw[n][0][3]),
                 color=(0, 0, 255),
                 thickness=1,
                 lineType=cv2.LINE_AA,
                 shift=0
                 )
        return draw_line
    else:
        print('No blue line')
        # 由于更改了摄像头位置，图片也更改了尺寸
        draw_line = np.zeros((240, 640, 3), np.uint8)
        return draw_line


# 这里想办法找到蓝线的两个端点坐标
def find_blue_line_give_points(blue_img):
    # 这个是定义检测直线精度的变量，默认值为1，越大，精度越底
    global K_blue
    # 这里设定检测直线所用到的累加器阈值，越大，则检测到的直线越长
    global threshold_2
    # 这个是存储的检测到的蓝线的过去的存储值

    # 通过HSV空间获取蓝色掩膜
    blue_line_mask = find_blue_line_mask(blue_img)
    cv2.imshow('blue_line_mask:', blue_line_mask)

    # 使用Canny算法进行边缘提取，这里设定的阈值就是(40,80)
    # canny_get = cv2.Canny(image=blue_line_mask,
    #                       threshold1=40,
    #                       threshold2=80,
    #                       apertureSize=3
    #                       )
    # # cv2.imshow('Canny:', canny_get)

    # 经过我的测试先废除canny检测
    canny_get = blue_line_mask

    line_2 = cv2.HoughLinesP(image=canny_get,
                             rho=1 * K_blue,
                             theta=np.pi / 180 * K_blue,
                             threshold=threshold_2,
                             minLineLength=200,
                             maxLineGap=30)

    if line_2 is not None:
        print('find_blue_line')
        # 设置进行操作的线条的序号，默认为0即可，有多条线可以更改序号
        n = 0
        points_1 = (line_2[n][0][0], line_2[n][0][1])
        points_2 = (line_2[n][0][2], line_2[n][0][3])

        return points_1, points_2, line_2

    else:
        print('None_line')
        # 没有检测到直线的情况
        # 就返回一个空值
        points_1_none = (0, 0)
        points_2_none = (0, 0)
        blue_line_none = None
        return points_1_none, points_2_none, blue_line_none


# 通过端点得到斜率
def get_slope(point1, point2):
    if point1[0] > point2[0]:
        k = (float(point1[1]) - float(point2[1])) / (float(point1[0] - point2[0]))
        angle_blue = - math.atan(k) / np.pi * 180
        print('angle_blue:', angle_blue)
        return angle_blue

    elif point2[0] > point1[0]:
        k = (float(point1[1]) - float(point2[1])) / (float(point1[0] - point2[0]))
        angle_blue = - math.atan(k) / np.pi * 180
        print('angle_blue:', angle_blue)
        return angle_blue

    else:
        print("slope is zero,angle_blue return None")
        angle_blue = None
        return angle_blue


# 9.14增加了获得中点距离的函数
# 得到中点的相关距离
def mid_points_distance(points_1, points_2):
    points3_x = (float(points_1[0]) + float(points_2[0])) / 2
    points3_y = (float(points_1[1]) + float(points_2[1])) / 2
    points3 = (points3_x, points3_y)

    print("points3_y:", points3_y)

    return points3_y


if __name__ == '__main__':

    cap_camera_wig = cv2.VideoCapture(0, cv2.CAP_DSHOW)
    init_detect()

    # 进入显示视频帧的循环
    while True:

        # 使用read这个函数读取视频帧，  返回bool值和   Mat图片数据
        # ret  //是否读取到数据
        # frame  //读取到的Mat数据
        ret1, frame1 = cap_camera_wig.read()
        # ret2, frame2 = cap_video_wig.read()

        # 这里将获取到的摄像头视频帧进行一个翻转操作(这个操作是后面添加上去的，可以随需求进行调整)
        # frame1 = cv2.flip(frame1, flipCode=1)

        # 如果读取到了，打印日志
        if ret1:

            # 这里进行摄像头的矫正操作
            # und_st = Straighten_the_camera(frame1)
            # und_st = frame1
            # 这里选取摄像头视频帧的下班部分进行切割
            und_st = frame1[240:, :]
        else:
            print('can get pic')
            sys.exit()

        points1, points2, line = find_blue_line_give_points(und_st)

        line_img = draw_line_2(und_st, line)

        get_slope(points1, points2)

        # 在nano里面需要删掉
        # print(f'points:({points1}, {points2})')

        cv2.imshow('original', und_st)
        cv2.imshow('draw_line:', line_img)

        # 等待10ms看有没有按键按下，读取指令
        key_wig = cv2.waitKey(1)

        # 这里设置了退出的指令
        if (key_wig & 0xFF) == ord('q'):
            print("Exit the programme")
            break
